
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_reverse_thrust.c
  * @author     baiyang
  * @date       2023-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
  return true if we are configured to support reverse thrust
 */
bool fms_have_reverse_thrust(void)
{
    return fms.aparm.throttle_min < 0;
}

/*
  see if reverse thrust should be allowed in the current flight state
 */
bool fms_allow_reverse_thrust(void)
{
    // check if we should allow reverse thrust
    bool allow = false;

    if (fms.g.use_reverse_thrust == USE_REVERSE_THRUST_NEVER || !fms_have_reverse_thrust()) {
        return false;
    }

    switch (mode_number(fms.control_mode)) {
    case MN_AUTO:
        {
        uint16_t nav_cmd = mission_get_current_nav_cmd()->id;

        // never allow reverse thrust during takeoff
        if (nav_cmd == MAV_CMD_NAV_TAKEOFF) {
            return false;
        }

        // always allow regardless of mission item
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_ALWAYS);

        // landing
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LAND_APPROACH) &&
                (nav_cmd == MAV_CMD_NAV_LAND);

        // LOITER_TO_ALT
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LOITER_TO_ALT) &&
                (nav_cmd == MAV_CMD_NAV_LOITER_TO_ALT);

        // any Loiter (including LOITER_TO_ALT)
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LOITER_ALL) &&
                    (nav_cmd == MAV_CMD_NAV_LOITER_TIME ||
                     nav_cmd == MAV_CMD_NAV_LOITER_TO_ALT ||
                     nav_cmd == MAV_CMD_NAV_LOITER_TURNS ||
                     nav_cmd == MAV_CMD_NAV_LOITER_UNLIM);

        // waypoints
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_WAYPOINT) &&
                    (nav_cmd == MAV_CMD_NAV_WAYPOINT ||
                     nav_cmd == MAV_CMD_NAV_SPLINE_WAYPOINT);

        // we are on a landing pattern
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_AUTO_LANDING_PATTERN) &&
                mission_get_in_landing_sequence_flag();
        }
        break;

    case MN_LOITER:
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_LOITER);
        break;
    case MN_RTL:
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_RTL);
        break;
    case MN_CIRCLE:
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_CIRCLE);
        break;
    case MN_CRUISE:
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_CRUISE);
        break;
    case MN_FLY_BY_WIRE_B:
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_FBWB);
        break;
    case MN_AVOID_ADSB:
    case MN_GUIDED:
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_GUIDED);
        break;
    case MN_TAKEOFF:
        allow = false;
        break;
    case MN_FLY_BY_WIRE_A:
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_FBWA);
        break;
    case MN_ACRO:
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_ACRO);
        break;
    case MN_STABILIZE:
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_STABILIZE);
        break;
    case MN_THERMAL:
        allow |= (fms.g.use_reverse_thrust & USE_REVERSE_THRUST_THERMAL);
        break;
    default:
        // all other control_modes allow independent of mask(MANUAL)
        allow = true;
        break;
    }

    // cope with bitwise ops above
    return allow != false;
}

/*
  return control in from the radio throttle channel.
 */
float fms_get_throttle_input(bool no_deadzone)
{
    float ret;
    if (no_deadzone) {
        ret = rc_get_control_in_zero_dz(fms.channel_throttle);
    } else {
        ret = rc_get_control_in(fms.channel_throttle);
    }
    if (fms.reversed_throttle) {
        // RC option for reverse throttle has been set
        ret = -ret;
    }
    return ret;
}

/*
  return control in from the radio throttle channel with curve giving mid-stick equal to TRIM_THROTTLE.
 */
float fms_get_adjusted_throttle_input(bool no_deadzone)
{
    if ((rc_get_type(fms.channel_throttle) != RC_CONTROL_TYPE_RANGE) ||
        (fms_flight_option_enabled(FO_CENTER_THROTTLE_TRIM)) == 0) {
       return  fms_get_throttle_input(no_deadzone);
    }
    float ret = rc_get_range(fms.channel_throttle) * math_throttle_curve(fms.aparm.throttle_cruise * 0.01f, 0, 0.5f + 0.5f*rc_norm_input(fms.channel_throttle ));
    if (fms.reversed_throttle) {
        // RC option for reverse throttle has been set
        return -ret;
    }
    return ret;
}
/*------------------------------------test------------------------------------*/


